﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Drawing.Imaging;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Diagnostics;
using System.Xml;
using System.Threading;
using System.Text.RegularExpressions;
using System.Runtime.InteropServices;
using System.IO;
using EARTHLib;
// CCR & DSS Namespaces
using Microsoft.Ccr.Core;
using Microsoft.Ccr.Adapters.WinForms;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Core.DsspHttp;

namespace JaguarControl
{
    public partial class MainForm : Form
    {
        #region Variables

        FromWinformsEvents _fromWinformPort;
        FromWinformMsg UpdatedData = new FromWinformMsg(FromWinformMsg.MsgEnum.Init);

        RobotConfig robotConfig = new RobotConfig();
        RobotConfig.RobotConfigTableRow jaguarSetting = null;
        private const string configFile = "c:\\DrRobotAppFile\\OutDoorRobotConfig.xml";
        private const string root = "C:\\Program Files (x86)\\Microsoft Robotics Dev Studio 2008 R3\\jcepeda\\CS\\Jaguar Control\\JaguarControl\\JaguarControl\\";
        public Process p1 = null;
        bool laserupdate = true;
        bool laserfrontview = false;
        bool manualcontrol = false;
        bool connectionsuccess = false;


        #region For Google Earth application

        public int gps_displayCnt = 0;
        public const int GPS_DISPER = 5;
        public int gps_t = 0;

        public bool LaunchGoogleEarth = false;
        [StructLayout(LayoutKind.Sequential)]
        public struct RECT
        {
            public int X;
            public int Y;
            public int Width;
            public int Height;
        }

        #region DLL Imports
        [DllImport("user32.dll", SetLastError = true)]
        static extern bool GetWindowRect(IntPtr hWnd, out RECT rect);

        public delegate int EnumWindowsProc(IntPtr hwnd, int lParam);

        [DllImport("user32", CharSet = CharSet.Auto)]
        public extern static IntPtr GetParent(IntPtr hWnd);

        [DllImport("user32", CharSet = CharSet.Auto)]
        public extern static bool MoveWindow(IntPtr hWnd, int X, int Y, int nWidth, int nHeight, bool bRepaint);

        [DllImport("user32", CharSet = CharSet.Auto)]
        public extern static IntPtr SetParent(IntPtr hWndChild, IntPtr hWndNewParent);

        [DllImport("user32.dll")]
        private static extern bool ShowWindowAsync(
        int hWnd,
        int nCmdShow);

        [DllImport("user32.dll", CharSet = CharSet.Auto)]
        public extern static bool SetWindowPos(int hWnd, IntPtr hWndInsertAfter, int X, int Y, int cx, int cy, uint uFlags);

        [DllImport("user32", CharSet = CharSet.Auto)]
        public extern static IntPtr PostMessage(int hWnd, int msg, int wParam, int IParam);
        #endregion

        readonly IntPtr HWND_BOTTOM = new IntPtr(1);
        readonly IntPtr HWND_NOTOPMOST = new IntPtr(-2);
        readonly IntPtr HWND_TOP = new IntPtr(0);
        readonly IntPtr HWND_TOPMOST = new IntPtr(-1);

        static readonly Int32 WM_QUIT = 0x0012;


        private IntPtr GEHrender = (IntPtr)0;
        private IntPtr GEParentHrender = (IntPtr)0;

        public ApplicationGEClass googleEarth;
        public CameraInfoGE cam = null;


        private string centerLatitude = "25.651455";
        private string centerLongitude = "-100.291642";  
        private int cameraRange = 200;
        private double preEstLatitude = 0;
        private double preEstLongitude = 0;

        private double preLatitude = 0;
        private double preLongtitude = 0;
        private double curLatitude = 0;
        private double curLongitude = 0;


        private string kmlFileStr1 = @"<?xml version=""1.0"" encoding=""utf-8""?><kml xmlns=""http://www.opengis.net/kml/2.2""><Document>";
        private string kmlFileStyle = @"<Style id=""MyLineStyle""><LineStyle><color>7f0000ff</color><width>4</width></LineStyle></Style>";


        private string kmlFilePlacemark = @"<Placemark><name>unextruded</name><styleUrl>#MyLineStyle</styleUrl><LineString><extrude>1</extrude><tessellate>1</tessellate>";
        private string kmlFileCoordinate = @"<coordinates>-79.3607666,43.855015,0 -79.3608,43.855015,0</coordinates>";
        private string kmlFileEnd = @"</LineString></Placemark></Document></kml>";

        private SetColor setColor = new SetColor();

        public class SetColor
        {
            public string SetRed = "0x7f0000ff";
            public string SetGreen = "0x7f00ff00";
            public string SetBlue = "0x7fff0000";
        }

        #endregion

        #region For Motors and Encoders
        public class MotorData
        {
            public int pwmOutput = 0;
            public int encoderSpeedPPS = 0;
            public double encoderSpeedRPM = 0;
            public double encoderSpeedMPS = 0;
            public int encoderPos = 0;
            public int encoderDir = 0;
            public double temperature = 0;
        }
        private MotorData leftFrontWheelMotor = new MotorData();
        private MotorData rightFrontWheelMotor = new MotorData();
        private MotorData leftRearWheelMotor = new MotorData();
        private MotorData rightRearWheelMotor = new MotorData();

        public struct EncoderRecord
        {
            public MotorData LeftFrontWheel;
            public MotorData LeftRearWheel;
            public MotorData RightFrontWheel;
            public MotorData RightRearWheel;
        }
        EncoderRecord encodersData = new EncoderRecord();

        //for temperature sensor
        private double[] resTable = new double[25] { 114660, 84510, 62927, 47077, 35563, 27119, 20860, 16204, 12683, 10000, 7942, 6327, 5074, 4103, 3336, 2724, 2237, 1846, 1530, 1275, 1068, 899.3, 760.7, 645.2, 549.4 };
        private double[] tempTable = new double[25] { -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, 100 };
        private const double FULLAD = 4095;
        private const double TEMPERATURE_TH = 65.0;
        private bool protectMotorTemp = true;

        //for stuck detection
        private double stuckVelTH = 50;
        private double stuckAcc = 0;
        private double stuckPWMTH = 6000;
        private double stuckAccTH = 25;
        private bool protectMotorStuck = true;

        private const int NOCONTROL = -32768;
        private bool lightOn = false;
        private int expandIO = 0xff;
        int speedFactor = 50;
        //CHANNELS 0,1,2 ARE NOT USED IN JAGUAR 4X4
        private const short LEFTCHANNEL = 3; //3  This channel changed with the dip switch change it was speed 
        private const short RIGHTCHANNEL = 4; //4 This channel changed with the dip switch change it was turn 
        
        //public static JoystickState[] joyState = new JoystickState[2]; //maybe we need second Joystick in future
        //private static int[] preSetButton = new int[2] { -1, -1 };  //no any button pressed
        //private static int[] joyButtonCnt = new int[2] { 0, 0 };
        //private const int JOYDELAY = 10;
        //private Device[] applicationDevice = new Device[2];
        //private bool blnJoyStick = true;
       
        private const int MAXPWM = 16384;
        private const int MINPWM = 4000;
        private const int INIPWM = 16384;
        private const int CMD_INT_TIME = 10;
        //private bool lightCtrl = true;
        private bool velInRPM = true;

        //vehicle constants in meters
        public const double WHEEL_RADIUS = 0.137;
        public const double WHEEL_DISTANCE = 0.426;
        #endregion 

        #region IMU 
        Image loadedImage1;
        Image loadedImage2;
        Image loadedImage3;
        private int calibrateCnt = 0;
        private double gyro_x_sum = 0;
        private double gyro_y_sum = 0;
        private double gyro_z_sum = 0;

        private int CALIBRATION_NUM = 20;

        private bool calibrateIMU = false;

        //For Drawings
        private double preYAW = 0;
        private double preROLL = 0;
        private double prePITCH = 0;
        private double preINDEX = 0;
        #endregion

        #endregion

        #region MainForm Operations
        public MainForm(FromWinformsEvents EventsPort)
        {
            _fromWinformPort = EventsPort;
            InitializeComponent();
            // Use this code for before connecting to robot for every groupbox disabling, those groups must be enabled 
            // once the robot is connected.
            groupboxCONTROLS.Enabled = false;
            groupboxBATT.Enabled = false;
            groupboxEARTH.Enabled = false;
            groupboxENCODERS.Enabled = false;
            groupboxIMU.Enabled = false;
            groupboxMAP.Enabled = false;
            groupboxPOSE.Enabled = false;
            groupboxSENSORS.Enabled = false;
        }

        private void MainForm_Load(object sender, EventArgs e)
        {
            #region Read Configuration XML
            robotConfig.Clear();
            try
            {
                robotConfig.ReadXml(configFile);
            }
            catch
            {
                MessageBox.Show("Unable to open robot configure file!", "DrRobot Jaguar Control", MessageBoxButtons.OK, MessageBoxIcon.Error);
                Close();
            }
            jaguarSetting = (RobotConfig.RobotConfigTableRow)robotConfig.RobotConfigTable.Rows[0];
            txtRobotID.Text = jaguarSetting.RobotID;
            txtRobotIP.Text = jaguarSetting.RobotIP;
            txtGPSIP.Text = jaguarSetting.GPSIP;
            txtCameraIP.Text = jaguarSetting.CameraIP;
            txtCameraPWD.Text = jaguarSetting.CameraPWD;
            txtCameraID.Text = jaguarSetting.CameraUser;
            #endregion
        }

        private void MainForm_FormClosed(object sender, EventArgs e)
        {
            myAMC.Stop();
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.Close));
        }

        private void MainForm_FormClosing(object sender, EventArgs e)
        {
            //Stop the robot
            if (connectionsuccess)
            {
                myJAGUAR.DcMotorPwmNonTimeCtr(LEFTCHANNEL, (short)INIPWM);
                myJAGUAR.DcMotorPwmNonTimeCtr(RIGHTCHANNEL, (short)INIPWM);
            }

            //Close WiRobotGateWay
            if (p1 != null)
                p1.CloseMainWindow();

            //Stop timers
            tmrDisplay.Enabled = false;
            //tmrJoyStickPoll.Enabled = false;

            //Stop communication links
            stopCommunication();
            stopCommunicationLaser();

            if ((startRecord) || (SW != null))
                SW.Close();

            #region Close GoogleEarth
            if (LaunchGoogleEarth)
            {
                double centerLat = double.Parse(centerLatitude);
                double centerLong = double.Parse(centerLongitude);
                try
                {
                    centerLat = double.Parse(txtStartLat.Text);
                }
                catch { }
                try
                {
                    centerLong = double.Parse(txtStartLong.Text);
                }
                catch { }

                jaguarSetting.GoogleEarthStartLat = centerLat;
                jaguarSetting.GoogleEarthStartLong = centerLong;
                try
                {
                    robotConfig.WriteXml(configFile);
                }
                catch { }
                try
                {
                    PostMessage(googleEarth.GetMainHwnd(), WM_QUIT, 0, 0);
                }
                catch { }
                //wait for Gooogle Earth close
                Thread.Sleep(1000);
            }
            #endregion
        }
        #endregion

        #region Sensor Events
        //Reads & Displays the battery levels for specific formatting when battery is low.
        private void myJaguar_StandardSensorEvent(object sender, EventArgs e)
        {
            connectionsuccess = true;
            double vol = (double)myJAGUAR.GetSensorBatteryAD2() / 4095 * 34.498;
            //to protect Li-Po Battery, you'd better shut down when battery is lower than 22.2V (3.7V*6)
            if (vol < 22.2 && vol >= 21.5)
            {
                textBATTERY.ForeColor = Color.Red;
                textBATTERY.Visible = !textBATTERY.Visible;
                textBATTERY.BackColor = Color.Yellow;
                UpdatedData.BatteryState = FromWinformMsg.BatteryEnum.LOW;
            }
            else if (vol >= 22.2)
            {
                textBATTERY.ForeColor = Color.Black;
                UpdatedData.BatteryState = FromWinformMsg.BatteryEnum.OK;
            }
            else if (vol < 21.5)
            {
                textBATTERY.ForeColor = Color.Red;
                textBATTERY.Visible = !textBATTERY.Visible;
                textBATTERY.BackColor = Color.Black;
                UpdatedData.BatteryState = FromWinformMsg.BatteryEnum.CRITICAL;
            }


            textBATTERY.Text = vol.ToString("0.00");
            
            //UpdateData
            UpdatedData.BatteryVoltage = vol;

            //here is the board voltage 5V, not display on the UI
            //vol = (double)myJAGUAR.GetSensorBatteryAD1() / 4095 * 9;
        }

        //Reads & Displays the encoder values and determines velocities.
        private void myJaguar_MotorSensorEvent(object sender, EventArgs e)
        {
            #region Mappings from myJaguar to MotorData variables

            /// GetDir -> 1,0 or -1 to indicate direction of rotation  (THE DIRECTION FOR RIGHT WHEELS HAS AN INVERTED SIGN)
            /// GetPulse -> Current pulse counter (0-32767) to indicate position of rotation
            /// GetSpeed -> Pulse change per second (187.76 ppm)
            /// PWMValue -> Same as GetPulse :: ONLY FRONT WHEELS PROVIDE A READING OF THE PWMOUTPUT, 
            //SINCE FRONT WHEELS ARE ALWAYS ASSIGNED EQUAL TO REAR WHEELS. 
            //SO, THE READING PERSISTS IN CHANNELS 4 AND 5.                     

            //Left Wheels
            leftFrontWheelMotor.encoderDir = myJAGUAR.GetEncoderDir4();
            leftFrontWheelMotor.encoderPos = myJAGUAR.GetEncoderPulse4();
            leftFrontWheelMotor.encoderSpeedPPS = myJAGUAR.GetEncoderSpeed4();
            leftFrontWheelMotor.pwmOutput = myJAGUAR.GetMotorPWMValue4();
            leftRearWheelMotor.encoderDir = myJAGUAR.GetEncoderDir1();
            leftRearWheelMotor.encoderPos = myJAGUAR.GetEncoderPulse1();
            leftRearWheelMotor.encoderSpeedPPS = myJAGUAR.GetEncoderSpeed1();
            leftRearWheelMotor.pwmOutput = myJAGUAR.GetMotorPWMValue4();

            //Rigth Wheels
            rightFrontWheelMotor.encoderDir = -myJAGUAR.GetEncoderDir5();
            rightFrontWheelMotor.encoderPos = myJAGUAR.GetEncoderPulse5();
            rightFrontWheelMotor.encoderSpeedPPS = myJAGUAR.GetEncoderSpeed5();
            rightFrontWheelMotor.pwmOutput = myJAGUAR.GetMotorPWMValue5();
            rightRearWheelMotor.encoderDir = -myJAGUAR.GetEncoderDir2();
            rightRearWheelMotor.encoderPos = myJAGUAR.GetEncoderPulse2();
            rightRearWheelMotor.encoderSpeedPPS = myJAGUAR.GetEncoderSpeed2();
            rightRearWheelMotor.pwmOutput = myJAGUAR.GetMotorPWMValue5();

            //Speed Equivalents  // 187.76666 are the pulses per revolution.
            leftFrontWheelMotor.encoderSpeedRPM = (leftFrontWheelMotor.encoderDir * leftFrontWheelMotor.encoderSpeedPPS * 60.0) / 187.766666;
            rightFrontWheelMotor.encoderSpeedRPM = (rightFrontWheelMotor.encoderDir * rightFrontWheelMotor.encoderSpeedPPS * 60.0) / 187.766666;
            leftRearWheelMotor.encoderSpeedRPM = (leftRearWheelMotor.encoderDir * leftRearWheelMotor.encoderSpeedPPS * 60.0) / 187.766666;
            rightRearWheelMotor.encoderSpeedRPM = (rightRearWheelMotor.encoderDir * rightRearWheelMotor.encoderSpeedPPS * 60.0) / 187.766666;
            leftFrontWheelMotor.encoderSpeedMPS = 2 * Math.PI * WHEEL_RADIUS / 60 * leftFrontWheelMotor.encoderSpeedRPM;
            rightFrontWheelMotor.encoderSpeedMPS = 2 * Math.PI * WHEEL_RADIUS / 60 * rightFrontWheelMotor.encoderSpeedRPM;
            leftRearWheelMotor.encoderSpeedMPS = 2 * Math.PI * WHEEL_RADIUS / 60 * leftRearWheelMotor.encoderSpeedRPM;
            rightRearWheelMotor.encoderSpeedMPS = 2 * Math.PI * WHEEL_RADIUS / 60 * rightRearWheelMotor.encoderSpeedRPM;


            //Assignment to variables for tests.
            encodersData.LeftFrontWheel = leftFrontWheelMotor;
            encodersData.LeftRearWheel = leftRearWheelMotor;
            encodersData.RightFrontWheel = rightFrontWheelMotor;
            encodersData.RightRearWheel = rightRearWheelMotor;

            #endregion

            #region Displays
            //Displaydata
            //Encoder Position
            lblEncoderPos1.Text = leftFrontWheelMotor.encoderPos.ToString();
            lblEncoderPos2.Text = rightFrontWheelMotor.encoderPos.ToString();
            lblEncoderPos3.Text = leftRearWheelMotor.encoderPos.ToString();
            lblEncoderPos4.Text = rightRearWheelMotor.encoderPos.ToString();

            //Encoder Velocity
            if (velInRPM)
            {
                lblVel1.Text = leftFrontWheelMotor.encoderSpeedRPM.ToString("0.00");
                lblVel3.Text = rightFrontWheelMotor.encoderSpeedRPM.ToString("0.00");
                lblVel2.Text = leftRearWheelMotor.encoderSpeedRPM.ToString("0.00");
                lblVel4.Text = rightRearWheelMotor.encoderSpeedRPM.ToString("0.00");
            }
            else
            {
                lblVel1.Text = leftFrontWheelMotor.encoderSpeedMPS.ToString("0.00");
                lblVel3.Text = rightFrontWheelMotor.encoderSpeedMPS.ToString("0.00");
                lblVel2.Text = leftRearWheelMotor.encoderSpeedMPS.ToString("0.00");
                lblVel4.Text = rightRearWheelMotor.encoderSpeedMPS.ToString("0.00");
            }


            //Encoder Direction
            dir1.Text = leftFrontWheelMotor.encoderDir.ToString();
            dir2.Text = leftRearWheelMotor.encoderDir.ToString();
            dir3.Text = rightFrontWheelMotor.encoderDir.ToString();
            dir4.Text = rightRearWheelMotor.encoderDir.ToString();

            //Encoder PWM Output
            pwm1.Text = leftFrontWheelMotor.pwmOutput.ToString();
            pwm2.Text = leftRearWheelMotor.pwmOutput.ToString();
            pwm3.Text = rightFrontWheelMotor.pwmOutput.ToString();
            pwm4.Text = rightRearWheelMotor.pwmOutput.ToString();
            #endregion

        }

        //Reads & Displays Temperatures and Motor Protects
        private void myJaguar_CustomSensorEvent(object sender, EventArgs e)
        {
            // For a motor, if output PWM is very big but encoder speed is very low or zero, it usually means the motor 
            // works in a stuck state, the current will be very big, the motor will be damaged in a moment, so please be 
            // very carefull at these information, especialy if temperature of a motor is getting much higher(above 60 degree), stop the motor for a while to protect the motor 
            //now the temperature is connect to Extended AD port

            #region Temperature
            double tempM1 = Trans2Temperature((double)myJAGUAR.GetCustomAD5());
            tempM1 = double.Parse(tempM1.ToString("0.00"));

            double tempM2 = Trans2Temperature((double)myJAGUAR.GetCustomAD6());
            tempM2 = double.Parse(tempM2.ToString("0.00"));

            double tempM3 = Trans2Temperature((double)myJAGUAR.GetCustomAD7());
            tempM3 = double.Parse(tempM3.ToString("0.00"));

            double tempM4 = Trans2Temperature((double)myJAGUAR.GetCustomAD8());
            tempM4 = double.Parse(tempM4.ToString("0.00"));

            encodersData.LeftFrontWheel.temperature = tempM1;
            leftFrontWheelMotor.temperature = tempM1;
            encodersData.LeftRearWheel.temperature = tempM2;
            leftRearWheelMotor.temperature = tempM2;
            encodersData.RightFrontWheel.temperature = tempM3;
            rightFrontWheelMotor.temperature = tempM3;
            encodersData.RightRearWheel.temperature = tempM4;
            rightRearWheelMotor.temperature = tempM4;

            if (checkBoxMotorProtect.Checked)
            {
                if ((tempM1 > TEMPERATURE_TH) || (tempM2 > TEMPERATURE_TH) || (tempM3 > TEMPERATURE_TH) || (tempM4 > TEMPERATURE_TH))
                {
                    protectMotorTemp = true;
                }
                else
                {
                    protectMotorTemp = false;
                }
            }
            else
            {
                protectMotorTemp = false;
            }

            if (tempM1 > TEMPERATURE_TH)
                lblTemp1.ForeColor = Color.Red;
            else
                lblTemp1.ForeColor = Color.Black;
            if (tempM2 > TEMPERATURE_TH)
                lblTemp2.ForeColor = Color.Red;
            else
                lblTemp2.ForeColor = Color.Black;
            if (tempM3 > TEMPERATURE_TH)
                lblTemp3.ForeColor = Color.Red;
            else
                lblTemp3.ForeColor = Color.Black;

            if (tempM4 > TEMPERATURE_TH)
                lblTemp4.ForeColor = Color.Red;
            else
                lblTemp4.ForeColor = Color.Black;

            lblTemp1.Text = tempM1.ToString("0.00");
            lblTemp2.Text = tempM2.ToString("0.00");
            lblTemp3.Text = tempM3.ToString("0.00");
            lblTemp4.Text = tempM4.ToString("0.00");
            #endregion

            #region Stuck Detection
            //stuck detect here
            if (
                (Math.Abs(myJAGUAR.GetMotorPWMValue4() - 16384) > stuckPWMTH &&
                        (Math.Abs(leftFrontWheelMotor.encoderSpeedPPS) < stuckVelTH ||
                         Math.Abs(leftRearWheelMotor.encoderSpeedPPS) < stuckVelTH)) ||
                (Math.Abs(myJAGUAR.GetMotorPWMValue5() - 16384) > stuckPWMTH &&
                        (Math.Abs(rightFrontWheelMotor.encoderSpeedPPS) < stuckVelTH ||
                         Math.Abs(rightRearWheelMotor.encoderSpeedPPS) < stuckVelTH)) ||
                (Math.Abs(leftFrontWheelMotor.encoderSpeedPPS - leftRearWheelMotor.encoderSpeedPPS) > 100) ||
                (Math.Abs(rightFrontWheelMotor.encoderSpeedPPS - rightRearWheelMotor.encoderSpeedPPS) > 100)
                )
            {
                stuckAcc++;
            }
            else
            {
                stuckAcc--;
                if (stuckAcc < 0) stuckAcc = 0;
            }
            if (stuckAcc >= stuckAccTH)
            {
                stuckAcc = stuckAccTH;
                pictureBoxSTUCK.BackColor = Color.Red;
                if (checkBoxMotorProtect.Checked)
                {
                    protectMotorStuck = true;
                    myJAGUAR.DcMotorPwmNonTimeCtr(RIGHTCHANNEL, INIPWM);
                    myJAGUAR.DcMotorPwmNonTimeCtr(LEFTCHANNEL, INIPWM);
                    trackBarSteerPower.Value = 0;
                    trackBarDrivePower.Value = 0;
                    trackBarLeftPower.Value = 0;
                    trackBarRightPower.Value = 0;
                }
            }
            else if (stuckAcc < stuckAccTH)
            {
                pictureBoxSTUCK.BackColor = Color.Green;
                if (checkBoxMotorProtect.Checked)
                {
                    protectMotorStuck = false;
                }
            }

            #endregion

            //Records: Encoders Position (LF,RF,LR,RR), IMU (Y,P,R), and GPS (alt,lat,long).
            #region Record Button actions
            string recTemp = "#M,";
            if (startRecord)
            {
                recTemp +=  encodersData.LeftFrontWheel.encoderPos.ToString() + "," + 
                            encodersData.RightFrontWheel.encoderPos.ToString() + "," +  
                            encodersData.LeftRearWheel.encoderPos.ToString() + "," + 
                            encodersData.LeftRearWheel.encoderPos.ToString() + "," +
                            
                            imuData.eYaw.ToString() + "," +
                            imuData.ePitch.ToString() + "," +
                            imuData.eRoll.ToString() + "," +
                            
                            gpsData.altitude.ToString() + "," +
                            gpsData.latitude.ToString() + "," +
                            gpsData.longitude.ToString();


                SW.WriteLine(recTemp);
                recordCnt++;
                if (recordCnt > MAXFILELEN)
                {
                    recordCnt = 0;
                    SW.Close();
                    //open next file

                    SW = File.CreateText(fileNme + fileCnt.ToString() + ".txt");
                    fileCnt++;

                }
            }
            #endregion

        }

        #endregion

        private void tmrDisplay_Tick(object sender, EventArgs e)
        {
            #region Draw IMU Graphs
            int dataLen = 0;
            if (drawEndPoint != drawStartPoint)
            {
                if (drawEndPoint > drawStartPoint)
                    dataLen = drawEndPoint - drawStartPoint;
                else
                    dataLen = DrawDataLen;

                DrawDataPic(Accel_Pitch, "Accel", Color.Crimson, draw_AccelX, drawStartPoint, drawEndPoint, dataLen, 512);
                DrawDataPic(Accel_Roll, "Accel", Color.Fuchsia, draw_AccelY, drawStartPoint, drawEndPoint, dataLen, 512);
                DrawDataPic(Accel_Yaw, "Accel", Color.DeepSkyBlue, draw_AccelZ, drawStartPoint, drawEndPoint, dataLen, 512);

                DrawDataPic(Gyro_Pitch, "Gyro", Color.Lime, draw_GyroY, drawStartPoint, drawEndPoint, dataLen, 512);
                DrawDataPic(Gyro_Roll, "Gyro", Color.DeepPink, draw_GyroX, drawStartPoint, drawEndPoint, dataLen, 512);
                DrawDataPic(Gyro_Yaw, "Gyro", Color.Blue, draw_GyroZ, drawStartPoint, drawEndPoint, dataLen, 512);
            }
            #endregion

            #region Draw IMU RPY
            //HERE WE MUST DRAW THE ROBOT THAT WILL ROTATE ACCORDING TO IMU READINGS
            if (imuData.index != preINDEX && loadedImage1 != null && loadedImage2 != null && loadedImage3 !=null)
            {
                preINDEX = imuData.index;
                if (Math.Abs(imuData.eYaw - preYAW) > 3)  //loadedImage2
                {
                    pictureBoxYAW.Image = RotateImage(loadedImage2, -imuData.eYaw);
                    pictureBoxYAW.Refresh();
                    preYAW = imuData.eYaw;
                }
                if (Math.Abs(imuData.eRoll - preROLL) > 3) //loadedImage1
                {
                    pictureBoxROLL.Image = RotateImage(loadedImage1, -imuData.eRoll);
                    pictureBoxROLL.Refresh();
                    preROLL = imuData.eRoll;
                }
                if (Math.Abs(imuData.ePitch - prePITCH) > 3)  //loadedImage3
                {
                    pictureBoxPITCH.Image = RotateImage(loadedImage3, -imuData.ePitch);
                    pictureBoxPITCH.Refresh();
                    prePITCH = imuData.ePitch;
                }
            }
            #endregion

            //Communicate data to service
            CommunicateData();
        }

        ///HERE WE LACK OF THE CODE FOR RELEASE CONTROL AND FOR MANUAL CONTROL 
        #region Buttons
        private void btnConnect_Click(object sender, EventArgs e)
        {
            #region Save Any Changes to ConfigFile
            //save any change to xml file
            jaguarSetting.LaserRangeIP = txtRobotIP.Text;

            jaguarSetting.RobotID = txtRobotID.Text;
            jaguarSetting.RobotIP = txtRobotIP.Text;
            jaguarSetting.GPSIP = txtGPSIP.Text;
            jaguarSetting.IMUIP = txtGPSIP.Text;
            jaguarSetting.CameraIP = txtCameraIP.Text;
            jaguarSetting.CameraPWD = txtCameraPWD.Text;
            jaguarSetting.CameraUser = txtCameraID.Text;

            robotConfig.WriteXml(configFile);
            #endregion

            #region Write Gateway ConfigFile
            //write gateway config file 
            XmlTextWriter textWriter = new XmlTextWriter("C:\\DrRobotAppFile\\gatewayConfig.xml", null);
            textWriter.WriteStartDocument();
            // Write comments
            textWriter.WriteComment("For Gateway Communication");
            // Write first element
            textWriter.WriteStartElement("GateWaySetting", "");
            // Write next element
            textWriter.WriteStartElement("RobotID", "");
            textWriter.WriteString(jaguarSetting.RobotID);
            textWriter.WriteEndElement();
            textWriter.WriteStartElement("RobotMethod", "");
            textWriter.WriteString("WiFi");
            //textWriter.WriteString("Serial");
            textWriter.WriteEndElement();
            textWriter.WriteStartElement("RobotCom", "");
            textWriter.WriteString("COM1");
            textWriter.WriteEndElement();
            textWriter.WriteStartElement("RobotIP", "");
            textWriter.WriteString(jaguarSetting.RobotIP);
            textWriter.WriteEndElement();
            textWriter.WriteStartElement("RobotPort", "");
            textWriter.WriteString("10001");
            textWriter.WriteEndElement();

            // Ends the document.
            textWriter.WriteEndElement();
            textWriter.WriteEndDocument();
            // close writer
            textWriter.Close();
            #endregion

            Thread.Sleep(1000);  //For ending the XML writing

            #region Start one WiRobotGateway
            p1 = new Process();
            p1.StartInfo.FileName = "C:\\DrRobotAppFile\\WirobotGateway.exe";
            p1.StartInfo.WindowStyle = ProcessWindowStyle.Minimized;
            p1.Start();
            #endregion

            Thread.Sleep(1000);  //For allowing robot connection

            #region Enable Groupboxes
            groupboxCONTROLS.Enabled = true;
            groupboxBATT.Enabled = true;
            groupboxEARTH.Enabled = true;
            groupboxENCODERS.Enabled = true;
            groupboxIMU.Enabled = true;
            groupboxMAP.Enabled = true;
            groupboxPOSE.Enabled = true;
            groupboxSENSORS.Enabled = true;
            #endregion

            Call4Connections();
            
            drawLaserBackground();
            drawMapBackground();
            loadIMUFigures();
            SetSpeedFactor(speedFactor);

        }

        #region GoogleEarth
        private void googleEarthEnable_Click(object sender, EventArgs e)
        {
            if (!LaunchGoogleEarth)
                LaunchGoogleEarth = true;

            centerLatitude = jaguarSetting.GoogleEarthStartLat.ToString();
            centerLongitude = jaguarSetting.GoogleEarthStartLong.ToString();

            txtStartLat.Text = centerLatitude;
            txtStartLong.Text = centerLongitude;

            if (this.DesignMode == false)
            {
                googleEarth = new ApplicationGEClass();

                GEHrender = (IntPtr)googleEarth.GetRenderHwnd();
                GEParentHrender = GetParent(GEHrender);

                MoveWindow(GEHrender, 0, 0, this.panelGE.Width, this.panelGE.Height, true);
                SetParent(GEHrender, this.panelGE.Handle);
            }
        }

        private void btnSetStartPoint_Click(object sender, EventArgs e)
        {
            cam = new CameraInfoGE();
            double centerLat = double.Parse(centerLatitude);
            double centerLong = double.Parse(centerLongitude);
            try
            {
                centerLat = double.Parse(txtStartLat.Text);
            }
            catch
            {
            }
            try
            {
                centerLong = double.Parse(txtStartLong.Text);
            }
            catch
            {
            }

            centerLat = (centerLat > 90 ? 90 : centerLat);
            centerLat = (centerLat < -90 ? -90 : centerLat);

            centerLong = (centerLong > 180 ? 180 : centerLong);
            centerLong = (centerLong < -180 ? -180 : centerLong);
            cam.FocusPointLatitude = centerLat;
            cam.FocusPointLongitude = centerLong;
            cam.FocusPointAltitude = 0;
            cam.Range = cameraRange;
            cam.Tilt = 0;


            googleEarth.SetCamera(cam, 0.6);
        }

        private void btnSetMapCenter_Click(object sender, EventArgs e)
        {
            PointOnTerrainGE pointGe = new PointOnTerrainGE();
            pointGe = googleEarth.GetPointOnTerrainFromScreenCoords(0, 0);


            double lat = pointGe.Latitude;
            double longitude = pointGe.Longitude;
            txtStartLat.Text = lat.ToString();
            txtStartLong.Text = longitude.ToString();

            btnSetStartPoint_Click(null, null);
        }

        //WE CAN ADD A TRACKBAR HERE FOR ZOOM IN GOOGLE EARTH
        //////////if (cam != null)
        //////////    {
        //////////       // cameraRange = trackBarZoom.Value;
        //////////        cam.Range = cameraRange;
        //////////        googleEarth.SetCamera(cam, 0.6);
        //////////    }

        #endregion

        #region Controls

        private void btnRELEASE_Click(object sender, EventArgs e)
        {
            manualcontrol = false;
        }


        private void btnSETMANUAL_Click(object sender, EventArgs e)
        {
            manualcontrol = true;
        }

        private void btnRecord_Click(object sender, EventArgs e)
        {
            if (btnRecord.Text == "RECORD")
            {
                btnRecord.Text = "STOP-REC";
                //fileCnt = 0; //don't reset the fileCnt
                recordCnt = 0;
                startRecord = true;
                SW = File.CreateText(fileNme + fileCnt.ToString() + ".txt");
                fileCnt++;
            }
            else
            {
                btnRecord.Text = "RECORD";
                startRecord = false;
                if (SW != null)
                    SW.Close();
            }
        }

        private void velSelector_Click(object sender, EventArgs e)
        {
            if (velSelector.Text == "Vel.(rpm)")
            {
                velSelector.Text = "Vel.(m/s)";
                velInRPM = false;
            }
            else
            {
                velSelector.Text = "Vel.(rpm)";
                velInRPM = true;
            }
        }

        private void btnResetMaxSpeed_Click(object sender, EventArgs e)
        {
            MaxSpeedFactor.Value = 50;
        }

        private void btnSTOPALL_Click(object sender, EventArgs e)
        {
            myJAGUAR.DcMotorPwmNonTimeCtr(LEFTCHANNEL, (short)INIPWM);
            myJAGUAR.DcMotorPwmNonTimeCtr(RIGHTCHANNEL, (short)INIPWM);
            trackBarSteerPower.Value = 0;
            trackBarDrivePower.Value = 0;
            trackBarLeftPower.Value = 0;
            trackBarRightPower.Value = 0;
        }

        private void MaxSpeedFactor_Scroll(object sender, EventArgs e)
        {
            speedFactor = MaxSpeedFactor.Value;
            SetSpeedFactor(speedFactor);
        }

        private void SetSpeedFactor(int newSpeedFactor)
        {
            trackBarDrivePower.Minimum = (-100 * newSpeedFactor) / 100;
            trackBarDrivePower.Maximum = (100 * newSpeedFactor) / 100;
            trackBarSteerPower.Minimum = (-100 * newSpeedFactor) / 100;
            trackBarSteerPower.Maximum = (100 * newSpeedFactor) / 100;
            trackBarRightPower.Minimum = (-100 * newSpeedFactor) / 100;
            trackBarRightPower.Maximum = (100 * newSpeedFactor) / 100;
            trackBarLeftPower.Minimum = (-100 * newSpeedFactor) / 100;
            trackBarLeftPower.Maximum = (100 * newSpeedFactor) / 100;
        }

        private void checkBoxMotorProtect_CheckedChanged(object sender, EventArgs e)
        {
            protectMotorStuck = false;
            protectMotorTemp = false;
        }

        private void btnLIGHTS_Click(object sender, EventArgs e)
        {
            if (lightOn)
            {
                expandIO = expandIO & 0x7f;     //bit 7 turn off lights
                lightOn = false;
                pictureBoxLIGHT1.BackColor = Color.LightGray;
                pictureBoxLIGHT2.BackColor = Color.LightGray;
                myJAGUAR.SetCustomDOUT((short)expandIO);
            }
            else
            {
                expandIO = expandIO | 0x80;     //bit 7 turn on lights
                lightOn = true;
                pictureBoxLIGHT1.BackColor = Color.Yellow;
                pictureBoxLIGHT2.BackColor = Color.Yellow;
                myJAGUAR.SetCustomDOUT((short)expandIO);
            }
        }
        #endregion

        #region Laser
        private void laserswitchview_Click(object sender, EventArgs e)
        {
            laserfrontview = !laserfrontview;
            if (!laserfrontview) drawLaserBackground();
        }

        #endregion

        #region IMU
        private void btnCalibrate_Click(object sender, EventArgs e)
        {
            gyro_x_sum = 0;
            gyro_y_sum = 0;
            gyro_z_sum = 0;

            calibrateCnt = 0;
            calibrateIMU = true;
        }
        #endregion

        #endregion

        #region Tracks
        private void trackBarLeftPower_Scroll(object sender, EventArgs e)
        {
            int forwardPWM = 0;
            if ((!protectMotorTemp) && (!protectMotorStuck))
            {
                if (trackBarLeftPower.Value != 0)
                {
                    forwardPWM = Math.Sign(trackBarLeftPower.Value) * ((int)((double)Math.Abs(trackBarLeftPower.Value) / 100 * (MAXPWM - MINPWM) + MINPWM));
                    forwardPWM = forwardPWM + INIPWM;
                }
                else
                    forwardPWM = INIPWM;

                if (forwardPWM > 32767) forwardPWM = 32767;
                if (forwardPWM < 0) forwardPWM = 0;
            }
            else
            {
                forwardPWM = INIPWM;
            }
            myJAGUAR.DcMotorPwmNonTimeCtr(LEFTCHANNEL, (short)forwardPWM);
        }

        private void trackBarRightPower_Scroll(object sender, EventArgs e)
        {
            int forwardPWM = 0;
            if ((!protectMotorTemp) && (!protectMotorStuck))
            {
                if (trackBarRightPower.Value != 0)
                {
                    forwardPWM = Math.Sign(trackBarRightPower.Value) * ((int)((double)Math.Abs(trackBarRightPower.Value) / 100 * (MAXPWM - MINPWM) + MINPWM));
                    forwardPWM = forwardPWM + INIPWM;
                }
                else
                    forwardPWM = INIPWM;

                if (forwardPWM > 32767) forwardPWM = 32767;
                if (forwardPWM < 0) forwardPWM = 0;
            }
            else
            {
                forwardPWM = INIPWM;
            }
            forwardPWM = 32767 - forwardPWM;
            myJAGUAR.DcMotorPwmNonTimeCtr(RIGHTCHANNEL, (short)forwardPWM);
        }

        private void trackBarDrivePower_Scroll(object sender, EventArgs e)
        {
            int forwardPWM = 0;
            if ((!protectMotorTemp) && (!protectMotorStuck))
            {
                if (trackBarDrivePower.Value != 0)
                {
                    forwardPWM = Math.Sign(trackBarDrivePower.Value) * ((int)((double)Math.Abs(trackBarDrivePower.Value) / 100 * (MAXPWM - MINPWM) + MINPWM));
                    forwardPWM = forwardPWM + INIPWM;
                }
                else
                    forwardPWM = INIPWM;

                if (forwardPWM > 32767) forwardPWM = 32767;
                if (forwardPWM < 0) forwardPWM = 0;
            }
            else
            {
                forwardPWM = INIPWM;
            }
            int forwardPWM_right = 32767 - forwardPWM;
            myJAGUAR.DcMotorPwmNonTimeCtr(LEFTCHANNEL, (short)forwardPWM);
            myJAGUAR.DcMotorPwmNonTimeCtr(RIGHTCHANNEL, (short)forwardPWM_right);
        }

        private void trackBarSteerPower_Scroll(object sender, EventArgs e)
        {
            int turnPWM = 0;
            if ((!protectMotorTemp) && (!protectMotorStuck))
            {
                if (trackBarSteerPower.Value != 0)
                {
                    turnPWM = Math.Sign(trackBarSteerPower.Value) * ((int)((double)Math.Abs(trackBarSteerPower.Value) / 100 * (MAXPWM - MINPWM) + MINPWM));
                    turnPWM = turnPWM + INIPWM;
                }
                else
                {
                    turnPWM = INIPWM;
                }

                if (turnPWM > 32767) turnPWM = 32767;
                if (turnPWM < 0) turnPWM = 0;
            }
            else
            {
                turnPWM = INIPWM;
            }
            myJAGUAR.DcMotorPwmNonTimeCtr(LEFTCHANNEL, (short)turnPWM);
            myJAGUAR.DcMotorPwmNonTimeCtr(RIGHTCHANNEL, (short)turnPWM);
        }

        private void trackBarZoom_Scroll(object sender, EventArgs e)
        {
            if (cam != null)
            {
                cameraRange = trackBarZoom.Value;
                cam.Range = cameraRange;
                googleEarth.SetCamera(cam, 0.6);
            }
        }
        #endregion

        #region Helpers

        #region Connections
        private void Call4Connections()
        {
            //WIROBOT ACTIVEX
            myJAGUAR.connectRobot(jaguarSetting.RobotID);

            #region AXISMEDIA ACTIVEX
            try
            {
                // Set properties, deciding what url completion to use by MediaType.
                myAMC.MediaUsername = jaguarSetting.CameraUser;
                myAMC.MediaPassword = jaguarSetting.CameraPWD;
                myAMC.MediaType = "mjpeg";
                myAMC.MediaURL = CompleteURL(jaguarSetting.CameraIP + ":" + jaguarSetting.CameraPort, "mjpeg");
                myAMC.AudioConfigURL = "http://" + jaguarSetting.CameraIP + ":" + jaguarSetting.CameraPort + "/axis-cgi/view/param.cgi?camera=1&action=list&group=Audio,AudioSource.A0";
                myAMC.ToolbarConfiguration = "default, + audiocontrol,+ rec";
                myAMC.AudioReceiveStop();
                // Start the streaming
                myAMC.Play();
                //Uncomment to activate audio
                //myAMC.AudioReceiveURL = "http://" + jaguarSetting.CameraIP + ":" + jaguarSetting.CameraPort + "/axis-cgi/audio/receive.cgi";
                //myAMC.Volume = 100;
                ////myAMC.AudioReceiveStart();
                //myAMC.AudioTransmitURL = "http://" + jaguarSetting.CameraIP + ":" + jaguarSetting.CameraPort + "/axis-cgi/audio/transmit.cgi";
            }
            catch (Exception ex)
            {
                MessageBox.Show(this, "Unable to play stream: " + ex.Message);
            }
            #endregion

            #region JOYSTICK
            ////for joystick control
            //if (InitDirectInput())
            //    tmrJoyStickPoll.Enabled = true;
            #endregion

            #region GPS, IMU and LRF

            //for GPS/IMU communication start
            if ((jaguarSetting.IMUIP != "0.0.0.0") && (jaguarSetting.GPSIP != "0.0.0.0"))
            {
                startComm();
            }

            // for laser scan start
            if ((jaguarSetting.LaserRangeIP != "0.0.0.0") && (jaguarSetting.LaserRangePort != 0) && laserupdate)
            {
                startCommLaser();
            }

            #endregion

        }

        private void loadIMUFigures()
        {
            loadedImage1 = Image.FromFile(root + "\\Resources\\Frontal.jpg");
            pictureBoxROLL.Image = loadedImage1;
            pictureBoxROLL.SizeMode = PictureBoxSizeMode.CenterImage;

            loadedImage2 = Image.FromFile(root + "\\Resources\\Superior.jpg");
            pictureBoxYAW.Image = loadedImage2;
            pictureBoxYAW.SizeMode = PictureBoxSizeMode.CenterImage;

            loadedImage3 = Image.FromFile(root + "\\Resources\\Lateral.jpg");
            pictureBoxPITCH.Image = loadedImage3;
            pictureBoxPITCH.SizeMode = PictureBoxSizeMode.CenterImage;

        }
        #endregion

        #region IP Validations
        /// <summary>
        /// method to validate an IP address
        /// using regular expressions. The pattern
        /// being used will validate an ip address
        /// with the range of 1.0.0.0 to 255.255.255.255
        /// </summary>
        /// <param name="addr">Address to validate</param>
        /// <returns></returns>
        public bool IsValidIP(string addr)
        {
            //create our match pattern
            string pattern = @"^([1-9]|[1-9][0-9]|1[0-9][0-9]|2[0-4][0-9]|25[0-5])(\.([0-9]|[1-9][0-9]|1[0-9][0-9]|2[0-4][0-9]|25[0-5])){3}$";
            //create our Regular Expression object
            Regex check = new Regex(pattern);
            //boolean variable to hold the status
            bool valid = false;
            //check to make sure an ip address was provided
            if (addr == "")
            {
                //no address provided so return false
                valid = false;
            }
            else
            {
                //address provided so use the IsMatch Method
                //of the Regular Expression object
                valid = check.IsMatch(addr, 0);
            }
            //return the results
            return valid;
        }

        private void txtRobotIP_Leave(object sender, EventArgs e)
        {
            string addre = txtRobotIP.Text;
            if (!IsValidIP(addre))
            {
                MessageBox.Show("This is an invalid address!", "Jaguar Control", MessageBoxButtons.OK, MessageBoxIcon.Error);
                txtRobotIP.Focus();
            }
        }

        private void txtGPSIP_Leave(object sender, EventArgs e)
        {
            string addre = txtGPSIP.Text;
            if (!IsValidIP(addre))
            {
                MessageBox.Show("This is an invalid address!", "Jaguar Control", MessageBoxButtons.OK, MessageBoxIcon.Error);
                txtGPSIP.Focus();
            }
        }

        private void txtCameraIP_Leave(object sender, EventArgs e)
        {
            string addre = txtCameraIP.Text;
            if (!IsValidIP(addre))
            {
                MessageBox.Show("This is an invalid address!", "Jaguar Motion/Power Control", MessageBoxButtons.OK, MessageBoxIcon.Error);
                txtCameraIP.Focus();
            }
        }

        #endregion

        #region Axis Media
        string CompleteURL(string theMediaURL, string theMediaType)
        {
            string anURL = theMediaURL;
            if (!anURL.EndsWith("/")) anURL += "/";

            if (theMediaType == "mjpeg")
            {
                anURL += "axis-cgi/mjpg/video.cgi";
            }
            else if (theMediaType == "mpeg4")
            {
                anURL += "mpeg4/media.amp";
            }
            else if (theMediaType == "mpeg2-unicast")
            {
                anURL += "axis-cgi/mpeg2/video.cgi";
            }
            else if (theMediaType == "mpeg2-multicast")
            {
                anURL += "axis-cgi/mpeg2/video.cgi";
            }

            anURL = CompleteProtocol(anURL, theMediaType);
            return anURL;
        }

        string CompleteProtocol(string theMediaURL, string theMediaType)
        {
            if (theMediaURL.IndexOf("://") >= 0) return theMediaURL;

            string anURL = theMediaURL;

            if (theMediaType == "mjpeg")
            {
                anURL = "http://" + anURL;
            }
            else if (theMediaType == "mpeg4")
            {
                anURL = "axrtsphttp://" + anURL;
            }
            else if (theMediaType == "mpeg2-unicast")
            {
                anURL = "http://" + anURL;
            }
            else if (theMediaType == "mpeg2-multicast")
            {
                anURL = "axsdp" + anURL;
            }

            return anURL;
        }
        #endregion

        #region Numerical
        private double Trans2Temperature(double adValue)
        {
            //for new temperature sensor
            double tempM = 0;
            double k = (adValue / FULLAD);
            double resValue = 0;
            if (k != 1)
            {
                resValue = 10000 * k / (1 - k);      //AD value to resistor
            }
            else
            {
                resValue = resTable[0];
            }


            int index = -1;
            if (resValue >= resTable[0])       //too lower
            {
                tempM = -20;
            }
            else if (resValue <= resTable[24])
            {
                tempM = 100;
            }
            else
            {
                for (int i = 0; i < 24; i++)
                {
                    if ((resValue <= resTable[i]) && (resValue >= resTable[i + 1]))
                    {
                        index = i;
                        break;
                    }
                }
                if (index >= 0)
                {
                    tempM = tempTable[index] + (resValue - resTable[index]) / (resTable[index + 1] - resTable[index]) * (tempTable[index + 1] - tempTable[index]);
                }
                else
                {
                    tempM = 0;
                }

            }

            return tempM;
        }
        #endregion

        #region Drawing
        private Image RotateImage(Image inputImg, double degreeAngle)
        {
            
            //Corners of the image
            PointF[] rotationPoints = { new PointF(0, 0),
                                        new PointF(inputImg.Width, 0),
                                        new PointF(0, inputImg.Height),
                                        new PointF(inputImg.Width, inputImg.Height)};

            //Rotate the corners
            PointMath.RotatePoints(rotationPoints, new PointF(inputImg.Width / 2.0f, inputImg.Height / 2.0f), degreeAngle);

            //Get the new bounds given from the rotation of the corners
            //(avoid clipping of the image)
            Rectangle bounds = PointMath.GetBounds(rotationPoints);

            //An empy bitmap to draw the rotated image
            Bitmap rotatedBitmap = new Bitmap(bounds.Width, bounds.Height);

            using (Graphics g = Graphics.FromImage(rotatedBitmap))
            {
                g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;
                g.InterpolationMode = InterpolationMode.HighQualityBicubic;

                //Transformation matrix
                Matrix m = new Matrix();
                m.RotateAt((float)degreeAngle, new PointF(inputImg.Width / 2.0f, inputImg.Height / 2.0f));
                m.Translate(-bounds.Left, -bounds.Top, MatrixOrder.Append); //shift to compensate for the rotation

                g.Transform = m;
                g.DrawImage(inputImg, 0, 0);
            }
            return (Image)rotatedBitmap;
        }

        public static class PointMath
        {
            private static double DegreeToRadian(double angle)
            {
                return Math.PI * angle / 180.0;
            }

            public static PointF RotatePoint(PointF pnt, double degreeAngle)
            {
                return RotatePoint(pnt, new PointF(0, 0), degreeAngle);
            }

            public static PointF RotatePoint(PointF pnt, PointF origin, double degreeAngle)
            {
                double radAngle = DegreeToRadian(degreeAngle);

                PointF newPoint = new PointF();

                double deltaX = pnt.X - origin.X;
                double deltaY = pnt.Y - origin.Y;

                newPoint.X = (float)(origin.X + (Math.Cos(radAngle) * deltaX - Math.Sin(radAngle) * deltaY));
                newPoint.Y = (float)(origin.Y + (Math.Sin(radAngle) * deltaX + Math.Cos(radAngle) * deltaY));

                return newPoint;
            }

            public static void RotatePoints(PointF[] pnts, double degreeAngle)
            {
                for (int i = 0; i < pnts.Length; i++)
                {
                    pnts[i] = RotatePoint(pnts[i], degreeAngle);
                }
            }

            public static void RotatePoints(PointF[] pnts, PointF origin, double degreeAngle)
            {
                for (int i = 0; i < pnts.Length; i++)
                {
                    pnts[i] = RotatePoint(pnts[i], origin, degreeAngle);
                }
            }

            public static Rectangle GetBounds(PointF[] pnts)
            {
                RectangleF boundsF = GetBoundsF(pnts);
                return new Rectangle((int)Math.Round(boundsF.Left),
                                     (int)Math.Round(boundsF.Top),
                                     (int)Math.Round(boundsF.Width),
                                     (int)Math.Round(boundsF.Height));
            }

            public static RectangleF GetBoundsF(PointF[] pnts)
            {
                float left = pnts[0].X;
                float right = pnts[0].X;
                float top = pnts[0].Y;
                float bottom = pnts[0].Y;

                for (int i = 1; i < pnts.Length; i++)
                {
                    if (pnts[i].X < left)
                        left = pnts[i].X;
                    else if (pnts[i].X > right)
                        right = pnts[i].X;

                    if (pnts[i].Y < top)
                        top = pnts[i].Y;
                    else if (pnts[i].Y > bottom)
                        bottom = pnts[i].Y;
                }

                return new RectangleF(left,
                                      top,
                                     (float)Math.Abs(right - left),
                                     (float)Math.Abs(bottom - top));
            }
        }

        //private void DrawHeading()
        //{

        //    Bitmap bmp = new Bitmap(pictureBoxYAW.Width, pictureBoxYAW.Height);
        //    Graphics g = Graphics.FromImage(bmp);

        //    int width = bmp.Width - 1;
        //    int height = bmp.Height - 1;

        //    int radius = Math.Min(width, height);

        //    g.Clear(Color.Transparent);
        //    g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;
        //    GraphicsPath path = new GraphicsPath();
        //    path.AddEllipse(0, 0, radius, radius);

        //    PathGradientBrush pathBrush = new PathGradientBrush(path);
        //    pathBrush.CenterPoint = new PointF(width / 3f, height / 3f);
        //    pathBrush.CenterColor = Color.White;
        //    pathBrush.SurroundColors = new Color[] { Color.LightBlue };

        //    g.FillPath(pathBrush, path);
        //    g.DrawPath(Pens.White, path);

        //    g.DrawArc(Pens.White, 0, 0, radius, radius, 0, 360);
        //    g.DrawArc(Pens.White, radius / 6, radius / 6, radius * 2 / 3, radius * 2 / 3, 0, 360);
        //    g.DrawArc(Pens.White, radius / 3, radius / 3, radius / 3, radius / 3, 0, 360);
        //    g.DrawLine(Pens.White, 0, radius / 2, radius, radius / 2);
        //    g.DrawLine(Pens.White, radius / 2, 0, radius / 2, radius);
        //    radius = radius / 2;
        //    g.DrawLine(Pens.White, (int)(radius - radius * Math.Cos(Math.PI / 3)), (int)(radius - radius * Math.Sin(Math.PI / 3)),
        //                (int)(radius + radius * Math.Cos(Math.PI / 3)), (int)(radius + radius * Math.Sin(Math.PI / 3)));
        //    g.DrawLine(Pens.White, (int)(radius - radius * Math.Cos(Math.PI / 6)), (int)(radius - radius * Math.Sin(Math.PI / 6)),
        //                (int)(radius + radius * Math.Cos(Math.PI / 6)), (int)(radius + radius * Math.Sin(Math.PI / 6)));
        //    g.DrawLine(Pens.White, (int)(radius - radius * Math.Cos(Math.PI / 3)), (int)(radius + radius * Math.Sin(Math.PI / 3)),
        //                (int)(radius + radius * Math.Cos(Math.PI / 3)), (int)(radius - radius * Math.Sin(Math.PI / 3)));
        //    g.DrawLine(Pens.White, (int)(radius - radius * Math.Cos(Math.PI / 6)), (int)(radius + radius * Math.Sin(Math.PI / 6)),
        //                (int)(radius + radius * Math.Cos(Math.PI / 6)), (int)(radius - radius * Math.Sin(Math.PI / 6)));
        //    radius = radius * 2;
        //    Font dirFont = new Font("Arial", 10, FontStyle.Bold);
        //    SolidBrush drawBrush = new SolidBrush(Color.Black);
        //    g.DrawString("N", dirFont, drawBrush, radius / 2 - 5, 0);
        //    g.DrawString("E", dirFont, drawBrush, radius - 12, radius / 2 - 6);
        //    g.DrawString("S", dirFont, drawBrush, radius / 2 - 5, radius - 12);
        //    g.DrawString("W", dirFont, drawBrush, 0, radius / 2 - 6);

        //    //draw target heading
        //    int x = 0;
        //    int y = 0;
        //    int r = radius - 4;

        //    try
        //    {

        //        x = (int)((radius / 2) + r / 2 * Math.Sin(imuData.eRoll*180/Math.PI));
        //        y = (int)(radius / 2 - r / 2 * Math.Cos(imuData.eRoll * 180 / Math.PI));
        //        Pen p1 = new Pen(Color.Red, 3);
        //        g.DrawLine(p1, x, y, radius+1 / 2, radius+1 / 2);
               
        //        pictureBoxYAW.Image = bmp;
        //    }
        //    catch
        //    {
        //    }

        //}

        private void DrawDataPic(PictureBox picCtrl, string dataName, Color penColor, double[] data, int startPos, int endPos, int length, int maxData)
        {
            Bitmap bmp = new Bitmap(picCtrl.Width, picCtrl.Height);
            Graphics g = Graphics.FromImage(bmp);

            int width = bmp.Width - 4;
            int height = bmp.Height - 4;
            Pen drawAxisPen = new Pen(Color.Black);
            Pen drawDataPen = new Pen(penColor);

            g.Clear(System.Drawing.Color.White);
            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;
            GraphicsPath path = new GraphicsPath();

            g.DrawLine(drawAxisPen, 0, height / 2, width, height / 2);
            int x = 0;
            int y = 0;
            int lastX = 0;
            int lastY = height / 2;
            double scaleY = ((double)height / 2) / maxData;
            double scaleX = (double)width / data.Length;
            int dataPos = 0;

            for (int i = 0; i < length; i++)
            {
                x = (int)(i * scaleX);

                dataPos = i + startPos;
                if (dataPos >= length)
                {
                    dataPos = dataPos - length;
                }
                y = (int)(data[dataPos] * scaleY) + height / 2;
                g.DrawLine(drawDataPen, lastX, lastY, x, y);
                lastX = x;
                lastY = y;
            }

            String drawString = dataName;
            // Create font and brush.
            Font drawFont = new Font("Arial", 7, FontStyle.Bold);
            SolidBrush drawBrush = new SolidBrush(Color.Red);
            // Create point for upper-left corner of drawing.
            float fontPosX = 3.0F;
            float fontPosY = 3.0F;
            // Set format of string.
            StringFormat drawFormat = new StringFormat();
            //drawFormat.FormatFlags = StringFormatFlags.DirectionVertical;

            // Draw string to screen.
            g.DrawString(drawString, drawFont, drawBrush, fontPosX, fontPosY, drawFormat);


            picCtrl.Image = bmp;
        }

        // Local copy of the image
        private Bitmap _MapImage;

        public Bitmap MapImage
        {
            get { return _MapImage; }
            set
            {
                _MapImage = value;

                Image old = picMap.Image;
                picMap.Image = value;

            }
        }

        #endregion

        #endregion

        private void CommunicateData()
        {
            if (connectionsuccess)
            {
                //UpdatedData.BatteryState and UpdatedData.BatteryVoltage are updated in myJaguar_StandardSensorEvent
                UpdatedData.EncodersData = encodersData;
                UpdatedData.GPSData = gpsData;
                UpdatedData.IMUData = imuData;
                UpdatedData.LaserData = laserData;
                UpdatedData.Map = (Bitmap)picMap.Image;
                UpdatedData.StuckDetect = protectMotorStuck;
                UpdatedData.TemperatureDetect = protectMotorTemp;
                UpdatedData.ManualControl = manualcontrol;
                //UpdatedData.CameraData:
                int size;
                object data;
                myAMC.GetCurrentImage(0, out data, out size);
                if (size > 0)
                {
                    // Create the bitmap from image data 
                    byte[] buffer = new byte[size];
                    Array.Copy((Array)data, (Array)buffer, size);
                    using (MemoryStream stream = new MemoryStream(buffer))
                    {
                        Image frame = Image.FromStream(stream);

                        UpdatedData.CameraData = frame;
                    }
                }

                //Post Message to Service
                _fromWinformPort.Post(UpdatedData);
            }
        }

        /// <summary>
        /// Sets a PWM value for the motor controller in the range [0,32767] from a -100 to 100 input value.
        /// </summary>
        /// <param name="leftspeed">integer from -100 to 100</param>
        /// <param name="rightspeed">integer from -100 to 100</param>
        public void SetSpeed(int leftspeed, int rightspeed)
        {
            if (leftspeed  < -100)    leftspeed  = -100;
            if (leftspeed   > 100)    leftspeed  =  100;
            if (rightspeed < -100)    rightspeed = -100;
            if (rightspeed  > 100)    rightspeed =  100;

            int leftPWM = 0;
            int rightPWM = 0;

            if ((!protectMotorTemp) && (!protectMotorStuck))
            {
                #region Set Left
                if (leftspeed != 0)
                {
                    leftPWM = Math.Sign(leftspeed) * ((int)((double)Math.Abs(leftspeed) / 100 * (MAXPWM - MINPWM) + MINPWM));
                    leftPWM = leftPWM + INIPWM;
                }
                else
                    leftPWM = INIPWM;

                if (leftPWM > 32767) leftPWM = 32767;
                if (leftPWM < 0) leftPWM = 0;
                #endregion 

                #region Set Right
                if (rightspeed != 0)
                {
                    rightPWM = Math.Sign(rightspeed) * ((int)((double)Math.Abs(rightspeed) / 100 * (MAXPWM - MINPWM) + MINPWM));
                    rightPWM = rightPWM + INIPWM;
                }
                else
                    rightPWM = INIPWM;

                rightPWM = 32767 - rightPWM;  //Right Speed is given in negatives because of right hand rule.

                if (rightPWM > 32767) rightPWM = 32767;
                if (rightPWM < 0) rightPWM = 0;
                #endregion 
            }
            else
            {
                leftPWM = INIPWM;
                rightPWM = INIPWM;
            }

            myJAGUAR.DcMotorPwmNonTimeCtr(LEFTCHANNEL, (short)leftPWM);
            myJAGUAR.DcMotorPwmNonTimeCtr(RIGHTCHANNEL, (short)rightPWM);
        }

    }
}

